#include "m3.h"

m3 m3::operator* (const m3 &M) const
{
	m3 Result;

	Result.x.x=x.x*M.x.x + y.x*M.x.y + z.x*M.x.z;
	Result.x.y=x.y*M.x.x + y.y*M.x.y + z.y*M.x.z;
	Result.x.z=x.z*M.x.x + y.z*M.x.y + z.z*M.x.z;

	Result.y.x=x.x*M.y.x + y.x*M.y.y + z.x*M.y.z;
	Result.y.y=x.y*M.y.x + y.y*M.y.y + z.y*M.y.z;
	Result.y.z=x.z*M.y.x + y.z*M.y.y + z.z*M.y.z;

	Result.z.x=x.x*M.z.x + y.x*M.z.y + z.x*M.z.z;
	Result.z.y=x.y*M.z.x + y.y*M.z.y + z.y*M.z.z;
	Result.z.z=x.z*M.z.x + y.z*M.z.y + z.z*M.z.z;

	Result.pos.x=M.pos.x*x.x + M.pos.y*y.x + M.pos.z*z.x + pos.x;
	Result.pos.y=M.pos.x*x.y + M.pos.y*y.y + M.pos.z*z.y + pos.y;
	Result.pos.z=M.pos.x*x.z + M.pos.y*y.z + M.pos.z*z.z + pos.z;
	return Result;
}


v3 m3::RotateOnlyVector(v3 const &V) const
{
	v3 R;

	R.x=
		x.x*V.x +
		y.x*V.y +
		z.x*V.z;
	R.y=
		x.y*V.x +
		y.y*V.y +
		z.y*V.z;
	R.z=
		x.z*V.x +
		y.z*V.y +
		z.z*V.z;

	return R;
}


v3 m3::ReverseRotateOnlyVector(v3 const &V) const
{
	v3 Copy;
	v3 Result;

	Copy=V;
	Result.x=Copy.Dot(x);
	Result.y=Copy.Dot(y);
	Result.z=Copy.Dot(z);

	return Result;
}




v3 m3::ReverseTransformVector(v3 const &V) const
{
	v3 Copy;
	v3 Result;

	Copy=V-pos;
	Result.x=Copy.Dot(x);
	Result.y=Copy.Dot(y);
	Result.z=Copy.Dot(z);

	return Result;
}


void m3::Normalize()
{
	if(z.MagnitudeSquared()==0.0f)
	{
		// Bad vector
		if(y.MagnitudeSquared()==0.0f)
		{
			y.Set(0.0f, 1.0f, 0.0f);
		}
		z=y.FindAbitraryPerpendicular();
	}
	else
	{
		z.Normalize();
		float f;
		f=z.Dot(y);
		if(OMath::FloatEquality(f, 0.0f))
		{
			// perp; done
		}
		else if(OMath::FloatEquality(f, 1.0f))
		{
			// parallel; error
			y=z.FindAbitraryPerpendicular();
		}
		else
		{
			v3 V;
			V=z.Cross(y);
			y=V.Cross(z);
			y.Normalize();
		}
	}
	x=y.Cross(z);
	x.Normalize();
}


void m3::Trans(const v3 &Translation)
{
	m3 M;
	M.SetTrans(Translation);
	*this=*this*M;
}


void m3::XRot(float fRadians)
{
	m3 M;
	M.SetXRot(fRadians);
	*this=*this*M;
}


void m3::YRot(float fRadians)
{
	m3 M;
	M.SetYRot(fRadians);
	*this=*this*M;
}


void m3::ZRot(float fRadians)
{
	m3 M;
	M.SetZRot(fRadians);
	*this=*this*M;
}


void m3::SetLookat(const v3 &Pos, const v3 &Target, const v3 &Up)
{
	SetYZPos(Up, Target-Pos, Pos);
}


void m3::Rot(const v3 &Axis, float fRadians)
{
	m3 M;
	M.SetRot(Axis, fRadians);
	*this=*this*M;
}


void m3::SetRot(const v3 &Axis, float fRadians)
{
	v3 NormAxis;

	NormAxis=Axis;
	NormAxis.Normalize();

	float fSin;
	float fCosine;
	float t,t1,t2;

	OMath::SinCos(fRadians, fSin, fCosine);
	t = 1.0f - fCosine;

	t1 = t*NormAxis.y;
	t2 = t*NormAxis.z;
	t  = t*NormAxis.x;

	x.Set(
		t * NormAxis.x + fCosine,
		t * NormAxis.y + fSin * NormAxis.z,
		t * NormAxis.z - fSin * NormAxis.y);
	y.Set(
		t1 * NormAxis.x - fSin * NormAxis.z,
		t1 * NormAxis.y + fCosine,
		t1 * NormAxis.z + fSin * NormAxis.x);
	z.Set(
		t2 * NormAxis.x + fSin * NormAxis.y,
		t2 * NormAxis.y - fSin * NormAxis.x,
		t2 * NormAxis.z + fCosine);
	pos=OMath::ZeroVector;
}


void m3::SetInverse(const m3 M)
{
	// NOTE - TRANSLATION AND ROTATION ONLY - DOESN'T HANDLE SCALING

	x.x=M.x.x;
	x.y=M.y.x;
	x.z=M.z.x;

	y.x=M.x.y;
	y.y=M.y.y;
	y.z=M.z.y;

	z.x=M.x.z;
	z.y=M.y.z;
	z.z=M.z.z;

  	pos.x=-M.pos.x*x.x - M.pos.y*y.x - M.pos.z*z.x;
	pos.y=-M.pos.x*x.y - M.pos.y*y.y - M.pos.z*z.y;
	pos.z=-M.pos.x*x.z - M.pos.y*y.z - M.pos.z*z.z;
}
